#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"

#if 1
#include "MPU6050/MPU6050.h"
#include "MPU6050/inv_mpu.h"
#include "MPU6050/inv_mpu_dmp_motion_driver.h"

static const char *TAG = "example";

void app_main(void)
{
    // uint8_t res = MPU_Init();
    // int16_t temp = 0;
    // int16_t ax = 0;
    // int16_t ay = 0;
    // int16_t az = 0;
    // while(1)
    // {
    //     ESP_LOGI(TAG, "MPU_Init = %d ", res);
    //     temp = MPU_Get_Temperature();
    //     MPU_Get_Accelerometer(&ax, &ay, &az);
    //     ESP_LOGI(TAG, "%d   %d %d %d", temp, ax, ay, az);
    //     vTaskDelay(1000 / portTICK_RATE_MS);
    // }

    uint8_t res = mpu_dmp_init();
    float pitch, roll,yaw; 

    while(1)
    {
        uint8_t res2 = mpu_dmp_get_data(&pitch, &roll, &yaw);
        
        printf("%d   %d\n", res, res2);
        if(res2 == 0)
        printf("pitch[%f] roll[%f] yaw[%f]\n", pitch, roll, yaw);

        vTaskDelay(50 / portTICK_RATE_MS);
    }
}

#else

#include "eMPL/MPU6050.h"
void app_main(void)
{
    MPU6050_Init();

    short accData[3];
    while(1)
    {
        MPU6050ReadAcc(accData);

        printf("%d %d %d\n", accData[0], accData[1], accData[2]);

        vTaskDelay(1000 / portTICK_RATE_MS);
    }
    
}

#endif // app_main
